from threading import Thread, Lock
import time
import math


from actuators.wheels import Wheels
from lookup_table import LookupTable
from Brain.vector import Vector
from Brain.plan import MovementStep
from Brain.sensors.encoder import EncoderResult
from Brain.operations.module import Module 


from Brain.recording import History, get_time_stamp, MovementRecord

pi_2 = 2.0 * math.pi 

# translate_lookup_table = LookupTable(
	# {
		# 18	: Vector(-1.0, 1.0), 
		# 10	: Vector(0.0, 1.0), 
		# 3	: Vector(0.7, 1.0), 
		# 2	: Vector(0.8, 1.0), 
		# 1	: Vector(0.9, 1.0), 
		# 0	: Vector(1.0, 1.0), 
		# -1	: Vector(1.0, 0.9), 
		# -2	: Vector(1.0, 0.8), 
		# -3	: Vector(1.0, 0.7), 
		# -10	: Vector(1.0, 0.0), 
		# -18	: Vector(1.0, -1.0), 
	# } )
	
## 
translate_lookup_table = LookupTable(
	{
#		18	: -15, 
#		10	: -10, 
		4	: 20, 
		2	: 9, 
		1	: 4, 
		0	: 0, 
		-1	: -4, 
		-2	: -9, 
		-4	: -20, 
#		-10	: 15, 
#		-18	: 15, 
	} )
translate_lookup_table.interval = math.pi / 80.0 #40.0


if 0:
	rotate_lookup_table = LookupTable(
		{
			9	: Vector(-0.5, 0.5), 
			3	: Vector(-0.2, 0.2), 
			2	: Vector(-0.1, 0.1), 
			1	: Vector(-0.05, 0.05), 
			0	: Vector(0.0, 0.0) , 
			-1	: Vector(0.05, -0.05), 
			-2	: Vector(0.1, -0.1), 
			-3	: Vector(0.2, -0.2), 
			-9	: Vector(0.5, -0.5), 
		} )
else:
	rotate_lookup_table = LookupTable(
		{
			3	: Vector(-0.1, 0.1), 
			1	: Vector(-0.05, 0.05), 
			0	: Vector(0.0, 0.0) , 
			-1	: Vector(0.05, -0.05), 
			-3	: Vector(0.1, -0.1), 
		} )
rotate_lookup_table.interval = math.pi / 40.0
	


def Angle(value):
	ret = abs(value) % pi_2
	if value < 0: return -ret
	return ret

def DirectionAngle( goal, current ):
	diff = Angle( goal - current )
	if diff > math.pi:
		diff -= pi_2
	elif diff < - math.pi:
		diff += pi_2
	return diff

def lookup_traversal_parameters(angle):
	""" returns (left power, right power, ) parameters for wheel controll
		negative angle indicates right turn result
	"""
	value = angle / translate_lookup_table.interval;
	#ret = translate_lookup_table[value].copy()
	return translate_lookup_table[value]
	
	#if value < 0.0: return Vector(ret.y, ret.x) 
	return ret
	
def lookup_rotate_parameters(angle):
	""" returns (left power, right power, ) parameters for wheel controll
		negative angle indicates right turn result
	"""
	value = angle / rotate_lookup_table.interval;
	ret = rotate_lookup_table[value].copy()
	#if value < 0.0: return Vector(-ret.x, -ret.y) 
	return ret 



class PlanStepInfo:
	""" plan step state structure
	"""
	PHASE_ROTATE = 1
	PHASE_TRANSLATE = 102
	
	def __init__(self, **kw):
		self.done = True
		#self.phase = self.PHASE_ROTATE
		self.phase = self.PHASE_TRANSLATE
		self.__dict__.update(kw)


class MotorCortex(Thread, Module):
	""" Movement logic
	"""
	SLEEP_INTERVAL = 0.01
	TRAVERSAL_TOLERANCE = 100.0
	ROTATE_TOLERANCE = math.pi / 36.0 #50.0 #36.0
	
	def __init__(self, world):
		Thread.__init__(self)
		Module.__init__(self)
		self._internal_resource = Lock()
		self._world = world 
		
		self._last_read = None
		self._sensor_max_reduction = 7
		self._sensor_reduction_index = 0
		
		self._current_pos = Vector( 0, 0 )
		self._current_velocity = Vector( 0, 0 )
		self._maneuver = None
		self._maneuver_state = PlanStepInfo()
		#self._encoder = encoder_device
		self._wheels = Wheels()
		self._sent_left, self._sent_right, = None, None, 
		
		## enable breaks
		self.stop()
		
		self.history = History('movement_history')
		self._cycles = 0
	
	def is_completed(self):
		return self._maneuver_state.done
	
	def kill_task(self): 
		""" abort further actions
		"""
		self._maneuver_state.done = True
		self.history.write( 'step interrupted ! ' )
	
	def start_task(self, step):
		""" start this maneuver 
			 + it is not advised to start new task before kill_task() for old task.
		"""
		self._maneuver = step.copy()
		self._maneuver_state = PlanStepInfo(done=False)
		self.history.write( 'execute new %s' % (str(step), ) )
	
	def stop(self):
		""" stop both motors 
		"""
		#self._wheels.write(0.0, 0.0)
		self._wheels.stop()
		self._sent_left, self._sent_right, = 0.0, 0.0, 
	
	def run(self):
		""" keep weels on track,
		"""
		self.stop()
		
		while self._working:
			self._run_logic()
			self._cycles += 1
			time.sleep(self.SLEEP_INTERVAL)
	
	def _run_logic(self):
		if not self._maneuver_state.done:
			## read sensors ..
			new = self._read_sensors()
			if new:
				if self._maneuver_state.phase == PlanStepInfo.PHASE_ROTATE:
					self._rotate_to_direction(new)
				elif self._maneuver_state.phase == PlanStepInfo.PHASE_TRANSLATE:
					self._linear_correction(new)
	
	def _read_sensors(self):
		""" removed read of encoders directly,
			data taken from world
		"""
		#data_read = self._encoder.read()
		data_read = EncoderResult( Vector(self._world.pos_x, self._world.pos_y), self._world.angle, Vector(self._world.velocity_x, self._world.velocity_y) )
		
		if data_read == self._last_read:
			self._sensor_reduction_index += 1 
			if self._sensor_reduction_index > self._sensor_max_reduction:
				self._sensor_reduction_index = 0 
			else:
				return None
		
		self._current_pos = data_read.pos
		self._current_velocity = data_read.velocity
		self._last_read = data_read.copy()
		return data_read
	
	def _send_to_actuators(self, param, my_angle, diff_angle, distance=0.0):
		""" pass new values to actuators
		"""
		## ZBUDZ
		self._wheels.write(param.x, param.y)
		self._sent_left, self._sent_right, = param.x, param.y, 
		self.history.write( MovementRecord(param, diff_angle, self._current_pos.copy(), my_angle, self._current_velocity.copy(), distance) )
	
	def _rotate_to_direction(self, data_read):
		""" apply rotation until targeted direction is acheived
		"""
		goal_direction = self._maneuver._destination - data_read.pos
		diff_angle = DirectionAngle(goal_direction.get_angle(), data_read.angle)
		if abs(diff_angle) < self.ROTATE_TOLERANCE:
			self.stop()
			self._maneuver_state.phase = PlanStepInfo.PHASE_TRANSLATE
			#self._maneuver_state.done = True 
			self.history.write( 'direction match, go forward.' )
		else:
			#print ' - diff:', diff_angle
			# send lookedup parameters to wheels
			self._send_to_actuators( lookup_rotate_parameters(diff_angle), data_read.angle, diff_angle )
	
	
	ROTATION_SWITCH_ANGLE = math.pi / 6.0
	MAX_SPEED = 20 #20 #30 # 20 #40 
	MIN_SPEED = 10 #15 #20 #40 
	TOP_SPEED_DISTANCE = MAX_SPEED / 500.0		# this should smooth movement 
	
	MAX_SPEED_LEFT = 20#40
	MAX_SPEED_RIGHT = 20#40
	MIN_SPEED_LEFT = 20#20
	MIN_SPEED_RIGHT = 20#20
	
	VUK = 0 
	def _linear_correction(self, data_read):
		""" keep robot in direction of targeted spot
		"""
		if self.VUK:
			goal_direction = self._maneuver._destination - data_read.pos
			alfa = goal_direction.get_angle() - data_read.angle
			dist = goal_direction.length()
			
			if dist < self.TRAVERSAL_TOLERANCE:
				##end of step
				self.stop()
				self._maneuver_state.done = True
				self.history.write( 'done.' )
				return 

			x = math.cos(alfa) * dist 
			y = math.sin(alfa) * dist 
			
			l = min( (x * 0.05) + (-y * 0.075), self.MAX_SPEED )
			d = min( (x * 0.05) + (y * 0.075), self.MAX_SPEED )
			if (-self.MIN_SPEED) > l < self.MIN_SPEED: l = 0 
			if (-self.MIN_SPEED) > d < self.MIN_SPEED: d = 0 
			if l < (-self.MAX_SPEED): l = -self.MAX_SPEED
			if d < (-self.MAX_SPEED): d = -self.MAX_SPEED
			
			self._wheels.send_raw(l, d)
			
			self.history.write( MovementRecord(
				(l, d), 0, self._current_pos.copy(), 
				data_read.angle, self._current_velocity.copy(), 0
				))
			print '--', int(l), int(d), (x, y,)
			
		else:
			goal_direction = self._maneuver._destination - data_read.pos
			diff_angle = DirectionAngle(goal_direction.get_angle(), data_read.angle)
			## check position, if current position in proximity step is finished
			distance = goal_direction.length()
			if distance < self.TRAVERSAL_TOLERANCE:
				## end of step
				self.stop()
				self._maneuver_state.done = True
				self.history.write( 'done.' )
			
			elif abs(diff_angle) > self.ROTATION_SWITCH_ANGLE :
				## switch to rotation ..
				self.stop()
				self._maneuver_state.phase = PlanStepInfo.PHASE_ROTATE
				self.history.write( 'angle to high, rotate.' )
			else:
				## send lookedup parameters to wheels
				diff = int(lookup_traversal_parameters(diff_angle))	# got right - left value 
				speed = int(min(self.MAX_SPEED, distance * self.TOP_SPEED_DISTANCE))
				if diff > 0: # apply reduction to left wheel
					left, right, = speed - diff, max(speed, self.MIN_SPEED_RIGHT),
				else: # apply reduction to right wheel 
					left, right, = max(speed, self.MIN_SPEED_LEFT), speed + diff,
				
				print diff, left, right 
				self._wheels.send_raw(left, right)
				#self._sent_left, self._sent_right, = left, right,
				self.history.write( MovementRecord(
					(left, right), diff_angle, self._current_pos.copy(), 
					data_read.angle, self._current_velocity.copy(), distance
					))
	
	def dump_logs(self):
		self.history.dump()
		now = time.strftime('%m-%d-%H%M')
		f = open( 'movement_%s.log' % (now, ), 'wb' )
		print ' - movement history - cycles: %s interval: %s' % (self._cycles, get_time_stamp(), ) 
		f.write(' - movement history - cycles: %s interval: %s' % (self._cycles, get_time_stamp(), ) + '\n')
		print '(timestamp, (left wheel, right wheel), direction angle differerence, (position) )' 
		for record in self.history.data: 
			f.write(str(record) + '\n')
		f.close()

global WORKING
WORKING = True

if __name__ == '__main__':
	from sensors.encoder import Encoder
	from Brain.world import WORLD 
	from Brain.brain import PositionSynchronization
	
	TESTING_INTERVAL = 20.0

	def KeyBreak():
		global WORKING
		data = raw_input()
		WORKING = False
	
	t = Thread(target = KeyBreak)
	t.setDaemon(True)
	
	encoder = Encoder()
	world = WORLD()
	kretanje = MotorCortex(world)
	
	updater = PositionSynchronization(world, encoder, None, None)
	
	updater.setDaemon(True)
	encoder.setDaemon(True)
	kretanje.setDaemon(True)
	kretanje.start()
	encoder.start()
	updater.start()
	
	print 'STOPING ..'
	encoder.reset(Vector(0, 0), 0)
	kretanje.stop()
	
	x = int(raw_input('destinacija x: '))
	y = int(raw_input('destinacija y: '))
	kretanje.start_task( MovementStep(x, y) )
	
	begin = time.time()
	from recording import mark_begining
	mark_begining()
	
	t.start()
	while WORKING:
		time.sleep(0.2)
		if kretanje.is_completed():
			print 'task completed ...'
			break
			
		if time.time() - begin > TESTING_INTERVAL:
			print ' ! break task ...'
			break
		
		print encoder.read()
	
	print 'dump logs : '
	kretanje.stop()
	kretanje.dump_logs()
	encoder.dump_logs()
	time.sleep(1)
	












